﻿using System;
using System.Collections;
using System.Collections.Generic;
using UnityEngine;
namespace GJKAlgorithm
{

    /// <summary>
    /// 2D版GJK碰撞检测库
    /// </summary>
    public static class GJK2DEngine
    {
        /// <summary>
        /// 迭代次数
        /// </summary>
        public static int LoopIterNum = 20;

        private const float Tolerance = 0.000001f;


        public struct SupportPoint
        {
            public Vector2 point; //明可夫斯基点
            public Vector2 fromA; //A集合点
            public Vector2 fromB; //B集合点

            public Vector2 GetPoint()
            {
                return point;
            }
        }


        public class Edge
        {
            public SupportPoint a; //a点
            public SupportPoint b;//b点
            public Vector2 normal;//边的法线方向
            public float distance;//边到原点距离
            public int index;//序号
        }

        public class SimplexEdge
        {
            public List<Edge> edges = new List<Edge>();

            public void clear()
            {
                edges.Clear();
            }

            public void initEdges(List<SupportPoint> simplex)
            {
                edges.Clear();

                int n = simplex.Count;
                for (int i = 0; i < n; ++i)
                {
                    int iNext = (i + 1) % n;
                    Edge edge = createEdge(simplex[i], simplex[iNext]);
                    edges.Add(edge);
                }

                updateEdgeIndex();
            }

            public Edge findClosestEdge()
            {
                float minDistance = float.MaxValue;
                Edge ret = null;
                foreach (var e in edges)
                {
                    if (e.distance < minDistance)
                    {
                        ret = e;
                        minDistance = e.distance;
                    }
                }
                return ret;
            }

            public void insertEdgePoint(Edge e, SupportPoint point)
            {
                Edge e1 = createEdge(e.a, point);
                edges[e.index] = e1;

                Edge e2 = createEdge(point, e.b);
                edges.Insert(e.index + 1, e2);

                updateEdgeIndex();
            }

            public void updateEdgeIndex()
            {
                for (int i = 0; i < edges.Count; ++i)
                {
                    edges[i].index = i;
                }
            }

            public Edge createEdge(SupportPoint a, SupportPoint b)
            {
                Edge e = new Edge();
                e.a = a;
                e.b = b;

                e.normal = GetPerpendicularToOrigin(a.point, b.point,Vector2.zero);
                float lengthSq = e.normal.sqrMagnitude;
                // 单位化边
                if (lengthSq > 0.0001f)
                {
                    e.distance = Mathf.Sqrt(lengthSq);
                    e.normal *= 1.0f / e.distance;
                }
                else
                {
                    // 如果距离原点太近，用数学的方法来得到直线的垂线
                    // 方向可以随便取，刚好另外一边是反着来的
                    Vector2 v = a.point - b.point;
                    v.Normalize();
                    e.normal = new Vector2(-v.y, v.x);
                }
                return e;
            }
        }

        /// <summary>
        /// 判断两个凸包围盒是否碰撞
        /// </summary>
        /// <param name="shapeA"></param>
        /// <param name="shapeB"></param>
        /// <returns></returns>
        public static bool IsContract(List<Vector2> shapeA, List<Vector2> shapeB, out Vector2 AClosestPonit, out Vector2 BClosestPoint, out List<SupportPoint> simplex,out Vector2 EPADir ,bool isCatchClosePoint = false)
        {
            EPADir = Vector2.zero;
            AClosestPonit = Vector2.zero;
            BClosestPoint = Vector2.zero;
            bool IsCollision = false;
            //List<SupportPoint> simplex = new List<SupportPoint
            simplex = new List<SupportPoint>();
            Vector2 direction = GetFirstDirection(0, 0, shapeA, shapeB);//获取方向向量
            simplex.Add(Support(direction, shapeA, shapeB));//获取该方向上第一个交集
            simplex.Add(Support(-direction, shapeA, shapeB));//获取第二个明克夫斯基差
            direction = -GetPerpendicularToOrigin(simplex[1].GetPoint(), simplex[0].GetPoint(), Vector2.zero);//指向原点的向量
            for (int i = 0; i < LoopIterNum; i++)
            {
                // 方向接近于0，说明原点就在边上
                if (direction.sqrMagnitude <= Tolerance)
                {
                    IsCollision = true;
                    break;
                }
                SupportPoint p = Support(direction, shapeA, shapeB);
                // 新点与之前的点重合了。也就是沿着dir的方向，已经找不到更近的点了。
                if ((p.GetPoint() - simplex[0].GetPoint()).sqrMagnitude < Tolerance || (p.GetPoint() - simplex[1].GetPoint()).sqrMagnitude < Tolerance)
                {
                    IsCollision = false;
                    break;

                }
                simplex.Add(p);//获取第三个明克夫斯基差

                //TODO 判断当前是否包含原点
                if (ContainPoint(simplex, Vector2.zero))
                {
                    IsCollision = true;
                    break;
                }
                //TODO 重新寻找离原点最近的两个点
                direction = FindNextDirection(simplex, shapeA, shapeB);//都是指向原点方向向量
            }

            if (!IsCollision && isCatchClosePoint)
            {
                GetCollisionPoint(simplex, ref AClosestPonit, ref BClosestPoint);
            }
            else
            {

               EPADir = QueryEPA(shapeA, shapeB,simplex);

            }
            return IsCollision;
        }

        private static Vector2 QueryEPA(List<Vector2> shapeA, List<Vector2> shapeB, List<SupportPoint> simplex)
        {
            SimplexEdge simplexEdge = new SimplexEdge();
            simplexEdge.initEdges(simplex);// 初始化边数据，// 构造一个首尾相连的多边形
            Vector2 penetrationVector = Vector2.zero;
            Edge currentEpaEdge = null;
            for (int i = 0; i < LoopIterNum; ++i)
            {
                Edge e = simplexEdge.findClosestEdge();//找到最近的边
                currentEpaEdge = e;
                if (e == null)
                {
                    return penetrationVector;
                }
                // 沿着边的法线方向，尝试找一个新的support点
                penetrationVector = e.normal * e.distance;
                // 沿着边的法线方向，尝试找一个新的support点
                SupportPoint point = Support(e.normal, shapeA, shapeB);
                float distance = Vector2.Dot(point.point, e.normal);
                if (distance - e.distance < Tolerance)
                {
                    // 返回穿透向量
                    return penetrationVector = e.normal * distance;
                }
                // 将新的support点插入到多边形中。
                // 也就是将边e从support点位置分割成两条新的边，并替换到多边形集合中。
                simplexEdge.insertEdgePoint(e, point);
            }
            return penetrationVector;
        }

        private static void GetCollisionPoint(List<SupportPoint> simplex, ref Vector2 closestOnA, ref Vector2 closestOnB)
        {
            /*
             *  L = AB，是Minkowski差集上的一个边，同时构成A、B两点的顶点也来自各自shape的边。
             *  E1 = Aa - Ba，E2 = Ab - Bb
             *  则求两个凸包的最近距离，就演变成了求E1和E2两个边的最近距离。
             *  
             *  设Q点是原点到L的垂点，则有:
             *      L = B - A
             *      Q · L = 0
             *  因为Q是L上的点，可以用r1, r2来表示Q (r1 + r2 = 1)，则有: Q = A * r1 + B * r2
             *      (A * r1 + B * r2) · L = 0
             *  用r2代替r1: r1 = 1 - r2
             *      (A - A * r2 + B * r2) · L = 0
             *      (A + (B - A) * r2) · L = 0
             *      L · A + L · L * r2 = 0
             *      r2 = -(L · A) / (L · L)
            */
            SupportPoint A = simplex[0];
            SupportPoint B = simplex[1];

            Vector2 L = B.GetPoint() - A.GetPoint();
            float sqrDistanceL = L.sqrMagnitude;
            // support点重合了
            if (sqrDistanceL < Tolerance)
            {
                closestOnA = closestOnB = A.GetPoint();
            }
            else
            {
                float r2 = -Vector2.Dot(L, A.GetPoint()) / sqrDistanceL;
                r2 = Mathf.Clamp01(r2);
                float r1 = 1.0f - r2;

                closestOnA = A.fromA * r1 + B.fromA * r2;
                closestOnB = A.fromB * r1 + B.fromB * r2;
            }
        }

        private static Vector2 FindNextDirection(List<SupportPoint> simplex, List<Vector2> shapeA, List<Vector2> shapeB)
        {

            if (simplex.Count == 2)
            {
                Vector2 crossPoint = GetPerpendicularToOrigin(simplex[0].GetPoint(), simplex[1].GetPoint(), Vector2.zero);
                return crossPoint;

            }
            else if (simplex.Count == 3)
            {
                Vector2 crossOnCA = GetPerpendicularToOrigin(simplex[2].GetPoint(), simplex[0].GetPoint(), Vector2.zero);
                Vector2 crossOnCB = GetPerpendicularToOrigin(simplex[2].GetPoint(), simplex[1].GetPoint(), Vector2.zero);
                // 保留距离原点近的，移除较远的那个点
                if (crossOnCA.sqrMagnitude < crossOnCB.sqrMagnitude)
                {
                    simplex.RemoveAt(1);
                    return Vector2.zero - crossOnCA;
                }
                else
                {
                    simplex.RemoveAt(0);
                    return Vector2.zero - crossOnCB;
                }
            }
            else
            {
                //error
                Debug.Log("FindNextDirection simplex count is beyond 3 !");
                return Vector2.zero;
            }

        }

        /// <summary>
        /// 寻找的原点到向量的垂足点位置
        /// </summary>
        private static Vector2 GetPerpendicularToOrigin(Vector2 a, Vector2 b, Vector2 original)
        {
            Vector2 ab = b - a;
            if (ab.sqrMagnitude == 0)
            {//a、b两点重合，随机返回
                return a;
            }
            Vector2 ao = original - a;
            float projection = Vector2.Dot(ab, ao) / ab.sqrMagnitude;
            if (projection < 0)//投影越界
            {
                return a;
            }
            else if (projection > 1)//投影越界
            {
                return b;

            }
            else
            {
                return a + ab * projection;
            }
        }

        /// <summary>
        /// 判断当前形状是否包含该点（三角形）
        /// </summary>
        /// <param name="shapePiontList"></param>
        /// <param name="point"></param>
        private static bool ContainPoint(List<SupportPoint> shapePiontList, Vector2 point)
        {
            int n = shapePiontList.Count;
            if (n < 3)
            {
                return false;
            }

            Vector2 a = shapePiontList[0].GetPoint();
            Vector2 b = shapePiontList[1].GetPoint();
            Vector2 c = shapePiontList[2].GetPoint();
            int inside = WhitchSide(a, b, c);
            for (int i = 0; i < shapePiontList.Count; ++i)
            {
                int side = WhitchSide(shapePiontList[i].GetPoint(), shapePiontList[(i + 1) % shapePiontList.Count].GetPoint(), point);
                if (side == 0)
                {
                    return true;
                }
                if (inside != side)
                {//点都必须在所有边的同一侧
                    return false;
                }
            }
            return true;
        }



        private static Vector2 GetFirstDirection(int indexA, int indexB, List<Vector2> a, List<Vector2> b)
        {
            Vector2 aP = a[indexA];
            Vector2 bP = b[indexB];
            Vector2 tempDir = aP - bP;
            if (tempDir.magnitude <= 0)
            {
                return GetFirstDirection(indexA + 1, indexB, a, b);
            }
            else
            {
                return tempDir;
            }
        }



        /// <summary>
        /// 获取明可夫斯基之差向量
        /// </summary>
        /// <returns></returns>
        private static SupportPoint Support(Vector2 dir, List<Vector2> A, List<Vector2> B)
        {
            Vector2 a = GetFarthestPointInDirection(dir, A);
            Vector2 b = GetFarthestPointInDirection(-dir, B);
            return new SupportPoint()
            {
                point = a - b,
                fromA = a,
                fromB = b
            };
        }

        private static Vector2 GetFarthestPointInDirection(Vector2 dir, List<Vector2> pointList)
        {

            int maxPointIndex = 0;
            float maxValue = float.MinValue;
            for (int i = 0; i < pointList.Count; i++)
            {
                float value = Vector2.Dot(pointList[i], dir);
                if (maxValue < value)
                {
                    maxValue = value;
                    maxPointIndex = i;
                }
            }
            return pointList[maxPointIndex];
        }


        /// <summary>
        /// 判断C点在a、b两点的哪一侧，利用叉乘来判断
        /// </summary>
        /// <param name="a"></param>
        /// <param name="b"></param>
        /// <param name="c"></param>
        /// <returns>0刚好在ab线上，1 在向量左边，2在向量右边</returns>
        public static int WhitchSide(Vector2 a, Vector2 b, Vector2 c)
        {
            Vector2 ab = b - a;
            Vector2 ac = c - a;
            float cross = ab.x * ac.y - ab.y * ac.x;
            return cross > 0 ? 1 : (cross < 0 ? -1 : 0);
        }
    }
}

